#include "tensorflowCCVisionLib.h"
#include "linemod_detector_Tool.h"
// #include "CsvData.h"
#include "opencv2/core.hpp"
#include "PclViewTools.h"

using namespace cv;
#include <string>
// #define VIDEO_TEST
#define image_tensor_width 480
#define image_tensor_height 270
// #define CSV_DataInfo
#define DEPTH_DEAL

std::vector<Eigen::Vector3f> CubeTranslationV;
std::vector<Eigen::Quaternionf> CubeRotationV;
std::vector<pose> outposedd;

void ScalarPoint(Point &p01, Point &p02, Point &p11, Point &p12, const Mat &DepthImg, const Mat &ColorImg);
void  viewerOneOff (pcl::visualization::PCLVisualizer& viewer);
void setPclPararm(std::vector<pose>& outposedd);
int main()
{

    vector<float> outPose;
    cv::Mat ColorImg, DepthImg;

    // TensorflowCCVisoin 初始化
    TensorflowCCVisoin *tensorTest = new TensorflowCCVisoin();
    int rtn = tensorTest->init(image_tensor_width, image_tensor_height);
    tensorTest->LoadModel("/home/de/testDatamnist/pb/frozen_inference_graph-20190715.pb");
    // tensorTest->LoadModel("");
    PclViewTools *PclViewTool = new PclViewTools();;
#ifdef DEPTH_DEAL
    /*********
     *  三维定位识别
     */
    LinemodDetector *LineDect = new LinemodDetector();
    // Mat xmlDepth, colorImg;
    // LineDect->loadData("/home/de/hirop_data/milk_big","milk","ply");
    LineDect->loadData("/home/de/hirop_data/milk_stl", "milk", "stl");
#endif

#ifdef CSV_DataInfo
    CsvData *csvData = new CsvData();
    csvData->Init("/home/de/testCode/tensorflowVision/CsvData/build/data.csv", {"image", "probabity", "size", "time"});
    vector<string> LogData;
    int videoIndex = 0;
#endif

#ifndef VIDEO_TEST
    std::string rootFolder = "/home/de/linemod-20190722测试样本/demo/";

    // = cv::imread("/home/de/build-20190716/ColorImg.png");
    // cv::Mat ColorImg = cv::imread("/home/de/linemod-20190722测试样本/demo/JPEGImages/143.png");;
    // cv::imwrite("/home/de/demo.jpg",ColorImg);
    for (int i = 0; i < 200; i++)
    {
        ColorImg = cv::imread(rootFolder + "JPEGImages/" + to_string(i) + ".png", IMREAD_UNCHANGED);
        cv::Mat temp = ColorImg.clone();

        cv::imwrite("/home/de/demo.jpg", ColorImg);
        ColorImg = imread("/home/de/demo.jpg");
        FileStorage fs(rootFolder + "depth/" + to_string(i) + ".xml", FileStorage::READ); //填读操作
        DepthImg.release();
        fs["depth"] >> DepthImg;
        fs.release();
        cout << "************************" << i << std::endl;
        cv::Mat tempDepthImg = DepthImg.clone();
        // cout << "channel "<<ColorImg.channels()<<endl;

        /******************************************** */
        //一定需要预处理图像,不然识别不准
        if (ColorImg.cols != image_tensor_width || ColorImg.rows != image_tensor_height)
        {
            resize(ColorImg, ColorImg, Size(image_tensor_width, image_tensor_height));
        }
        else
        {
            cout << "the image size is ok " << endl;
            cout << ColorImg.rows << " " << ColorImg.cols << endl;
        }

        //图像需要的是RGB的格式 不然的话会识别有误
        cvtColor(ColorImg, ColorImg, CV_BGR2RGB);
        if (ColorImg.empty() || DepthImg.empty())
        {
            perror("non image ");
            return -1;
        }
        cv::imshow("ColorImg", ColorImg);
        // cv::imshow("DepthImg",DepthImg);
        // cv::waitKey(0);

#endif
        /******************************************** */

        ///视频测试
#ifdef VIDEO_TEST
        cv::VideoCapture vc("/home/de/testCode/output.avi");
        while (vc.isOpened())
        {

#ifdef CSV_DataInfo
            videoIndex++;
            LogData.clear();
            LogData.reserve(4);
#endif
            cv::Mat Frame, Frame2;
            vc >> Frame2;
            std::cout << Frame2.channels() << " " << Frame2.depth() << endl;
            std::cout << Frame2.cols << " " << Frame2.rows << endl;
            //没有这里两个 resize cvtColor 预处理的话,会出现 误识别的情况.识别的通道数一定是 RGB
            resize(Frame2, Frame, Size(Frame2.cols / 2, Frame2.rows / 2));
            cvtColor(Frame, Frame, CV_BGR2RGB);
            ColorImg = Frame;
#endif

            outPose.clear();
            rtn = tensorTest->detection(ColorImg, outPose);

            if (rtn != 0)
            {
                printf("non found the target....... \n");
#ifdef CSV_DataInfo
                LogData.push_back(std::to_string(videoIndex));
                LogData.push_back(std::to_string(outPose[1]));
                LogData.push_back(std::to_string(0));
                LogData.push_back(std::to_string(0.2));
                csvData->WriteData(LogData);
#endif

#ifdef VIDEO_TEST
                continue;
#else
        continue;
        // return -1;
#endif
            }
            //cv::waitKey(0);

#ifdef CSV_DataInfo
            LogData.push_back(std::to_string(videoIndex));
            LogData.push_back(std::to_string(outPose[1]));
            int area = roiR.area();
            LogData.push_back(std::to_string(area));
            LogData.push_back(std::to_string(0.2));
            csvData->WriteData(LogData);
#endif

            for (vector<float>::iterator itor = outPose.begin(); itor != outPose.end(); itor++)
            {
                cout << " out pose " << *itor << endl;
            }
            Point2i p11(outPose[2], outPose[3]), p12(outPose[4], outPose[5]);
            cv::Rect RectDepth = Rect(p11, p12);
            cv::rectangle(ColorImg, RectDepth, Scalar(250, 0, 255), 1.5);

#ifndef VIDEO_TEST

            cout << DepthImg.rows << " " << DepthImg.cols << endl;

            Point2i p21, p22;
            ScalarPoint(p11, p12, p21, p22, DepthImg, ColorImg);
            // //后面的深度图分割需要采用  需求是深度图是1920*1080 而彩色图是480*270 需要比例放大
            RectDepth = Rect2i(p21, p22);
            //裁剪出来的图像
            cv::Mat RoiDepth = DepthImg(RectDepth);
            cv::Mat RoiColor = temp(RectDepth);

            //自己先扣ROI 出来
            cv::Mat InitDepthImg = cv::Mat::zeros(DepthImg.rows, DepthImg.cols, CV_16UC1);
            cv::Mat InitDepthRoiImg = InitDepthImg(RectDepth);
            cv::Mat InitColorImg = cv::Mat::zeros(temp.rows, temp.cols, CV_8UC3);
            cv::Mat InitColorRoiImg = InitColorImg(RectDepth);

            RoiDepth.copyTo(InitDepthRoiImg);
            RoiColor.copyTo(InitColorRoiImg);

            // imwrite("DepthImg.png", DepthImg);
            // imwrite("RoiDepth.png", RoiDepth);
            // imshow("srcDepth", DepthImg);
            // imshow("RoiDepth", RoiDepth);
            // imshow("RoiColor",RoiColor);

            //写入 测试数据集
            // FileStorage fs("/home/de/testDatamnist/tempDataDepth/depth/"+to_string(i)+"t.xml", FileStorage::WRITE); //填读操作
            // fs<<"depth"

            // cvtColor(ColorImg, ColorImg, CV_RGB2BGR);
#endif
            // imshow("ColorImg", ColorImg);
            // imshow("RoiColor",RoiColor);
            // cv::waitKey(0);

            // cv::waitKey(0);

#ifdef VIDEO_TEST
        }

#endif

#ifdef CSV_DataInfo
        csvData->close();
#endif

        // cv::imwrite("/home/de/testCode/PCDFILE/smallROIDepth1.png", RoiDepth);
#ifdef DEPTH_DEAL
        /*********
     *  三维定位识别
     */
        // LinemodDetector *LineDect = new LinemodDetector();
        // std::vector<pose> outposedd;
        // // Mat xmlDepth, colorImg;
        // LineDect->loadData("/home/de/hirop_data/milk_big","milk","ply");
        // FileStorage fs("/home/de/testCode/tensorflowVision/build/data/test.xml", FileStorage::READ); //填读操作
        // // FileStorage fs("test.yml", FileStorage::READ);
        // fs ["depth"]>>xmlDepth;
        // fs.release();
        // cout<<"xmlDepth: "<<xmlDepth.depth()<<endl;
        // colorImg = imread("/home/de/hirop_data/Renderer_src_1.png");

        outposedd.clear();
        // Rect re = Rect();
        LineDect->setDepthImg(InitDepthImg);
        LineDect->setColorImg(InitColorRoiImg);
        int rtn1 = LineDect->detection();
        if (rtn1 == 0)
        {
            LineDect->getResult(outposedd);
            imshow("InitDepthImg", InitDepthImg);
            imshow("InitColorImg", InitColorImg);
            cout << " find------------------------------! " << endl;
            
            
            setPclPararm(outposedd);
            PointCloud::Ptr pcloud(new PointCloud);
            pclViewTool->DepthToCloud(InitColorImg, InitDepthImg, pcloud);
            pcl::visualization::CloudViewer *viewer = new pcl::visualization::CloudViewer("Simple Cloud Viewer");
            viewer->runOnVisualizationThreadOnce(*viewerOneOff);
            viewer->showCloud(pcloud); // viewer.runOnVisualizationThreadOnce(viewerOneOff);     viewer.get()->removeAllPointClouds();

            while (!viewer->wasStopped())
            {
                // viewer->spinOnce(100);
                boost::this_thread::sleep(boost::posix_time::microseconds(1000));
            };
            delete viewer;
        }
        else
        {
            cout << "non find " << endl;
            // return -1;
        }

#ifndef VIDEO_TEST
    }
#endif

#endif
    delete PclViewTool;
    delete tensorTest;
    return 0;
}

/**
 * 深度图比例分割
 */
void ScalarPoint(Point &p01, Point &p02, Point &p11, Point &p12, const Mat &DepthImg, const Mat &ColorImg)
{

    float scalarX = static_cast<float>(DepthImg.cols) / ColorImg.cols;
    float scalarY = static_cast<float>(DepthImg.rows) / ColorImg.rows;
    p11.x = static_cast<int>(scalarX * p01.x);
    p12.x = static_cast<int>(scalarX * p02.x);

    p11.y = static_cast<int>(scalarY * p01.y);
    p12.y = static_cast<int>(scalarY * p02.y);
    cout << "scalarX: " << scalarX << endl;
    cout << "scalarY: " << scalarY << endl;
    cout << "p11 point :" << p11.x << " " << p11.y << endl;
    cout << "p12 point :" << p12.x << " " << p12.y << endl;
}


void  viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    // viewer.setBackgroundColor (1.0, 0.5, 1.0);

    // viewer.addSphere (o, 0.15, "sphere", 0);
    for( int i = 0; i < outposedd.size() ; i++){
    // for( int i = 0; i < 1 ; i++){
          std::string cubeName = "cube " + to_string(i);
          viewer.addCube(CubeTranslationV[i], CubeRotationV[i], 0.05,0.1,0.05,cubeName);
          // viewer.addCylinder
    }
    std::cout << "I only run once" << std::endl;
    
}


void setPclPararm(std::vector<pose>& outposedd){
    CubeTranslationV.clear();
    CubeRotationV.clear();
    for( int i = 0; i < outposedd.size() ; i++){
        Eigen::Vector3f CubeTranslation = Eigen::Vector3f();
         CubeTranslation<<outposedd[i].position.x,outposedd[i].position.y,outposedd[i].position.z;
        float changeQuater[4];
        changeQuater[0] = static_cast<float>(outposedd[i].quaternion.x);
        changeQuater[1] = static_cast<float>(outposedd[i].quaternion.y);
        changeQuater[2] = static_cast<float>(outposedd[i].quaternion.z);
        changeQuater[3] = static_cast<float>(outposedd[i].quaternion.w);
        cout << " changeQuater "<< changeQuater[0] << " "<<changeQuater[1]<< " "<<changeQuater[2]<< " "<<changeQuater[3]<<std::endl;
        // CubeTranslation<<0,0,0;
        Eigen::Quaternionf CubeRotation = Eigen::Quaternionf(changeQuater[3],changeQuater[0], changeQuater[1],\
                                            changeQuater[2]);
                                

        CubeTranslationV.push_back(CubeTranslation);
        CubeRotationV.push_back(CubeRotation);
        // std::string cubeName = "cube " + toString(i);
        // viewer->addCube(CubeTranslation, CubeRotation, 0.1,0.2,0.1,cubeName);
                                
    }

}
// void Scalar()